home *** CD-ROM | disk | FTP | other *** search
/ Languguage OS 2 / Languguage OS II Version 10-94 (Knowledge Media)(1994).ISO / language / embedded / 68hc11 / servo8_a.txt < prev    next >
Text File  |  1994-10-20  |  13KB  |  370 lines
  1. *
  2. *       Author: Alan Kilian   
  3. *
  4. *       Title : HC11 RC Servo routines
  5. *
  6. *       File Name : servo8.asm
  7. *
  8. *       Description : This program demonstrates the use of a HC11 processor
  9. *              to control 8 Model airplane type servos. The servos
  10. *              are updated every 20 milliseconds and their individual
  11. *              pulses do not jitter more than 1 microsecond.
  12. *
  13. *       History : 02/11/93 Created. Tested out one pulse.
  14. *          02/15/93 First cut of eight pulses.
  15. *          02/16/93 Getting eight good clean pulses.
  16. *          02/17/93 Added sort routine and tons of comments.
  17. *          02/17/93 Added user input routine.
  18. *          02/18/93 Debugged the whole thing and it was no fun at all
  19. *             02/18/93 Removed all the printing routines and debug stuff.
  20. *          02/20/93 Removed FCB from RAM area. Don't do that.
  21. *
  22. *    Use:    Send ASCII characters over the serial line to control the servos
  23. *        The format is list this:  
  24. *        s0000    sets servo 00 to pulse width $00 which is the minimum
  25. *             available pulse width.
  26. *        s0080    sets servo 00 to pulse width $80 HEX which is the
  27. *             middle width pulse
  28. *        s00FF    sets servo 00 to pulse width $FF HEX which is the 
  29. *              largest available pulse width
  30. *        s0100    servo one
  31. *        r        resets all servos to a pulse width $80
  32. *
  33. *       Note : This program is written for the MC68HC811E2 processor running
  34. *           in single-chip mode. It is designed for use with a processor
  35. *           running with an 8 mHz crystal. If you are using a different
  36. *           crystal frequency you will need to re-compute all of the
  37. *           timing values in this code.
  38. *
  39. *           The structure, serial I/O and command processor portions of
  40. *           this program evolved from the program HEXLOB40 written by
  41. *           Fred Martin and Randy Sargent and we thank them greatly.
  42. ****************************************************************************
  43.  
  44. * Two-Byte Registers (High,Low -- Use Load & Store Double to access)
  45. TOC2    EQU    $1018    ; Timer Output Compare register 2
  46.  
  47. * One-Byte Registers
  48. PORTB    EQU    $1004    ; PORT B data register
  49. TCTL1    EQU    $1020    ; Timer ConTroL register 1
  50. TCTL2    EQU    $1021    ; Timer ConTroL register 2
  51. TMSK1    EQU    $1022    ; Timer interrupt MaSK register 1
  52. TFLG1    EQU    $1023    ; Timer interrupt FLaG register 1
  53. RTFLG1    EQU    $23    ; Relative to $1000 Timer interrupt FLaG register 1
  54. TMSK2    EQU    $1024    ; Timer interrupt MaSK register 2
  55. TFLG2    EQU    $1025    ; Timer interrupt FLaG register 2
  56. SPCR    EQU    $1028    ; SPI Control Register
  57. BAUD    EQU    $102B    ; SCI Baud Rate Control Register
  58. SCCR2    EQU    $102D    ; SCI Control Register 2
  59. SCSR    EQU     $102E    ; SCI Status Register (Actual location)
  60. SCDR    EQU     $102F    ; SCI Data Register (Actual location)
  61.  
  62.  
  63. * Masks for serial port
  64. PORTD_WOM    EQU    $20    ; Wire-OR mode
  65. BAUD1200    EQU    $B3    ; 1200 Baud what else?
  66. BAUD9600    EQU    $B0    ; 9600 Baud what else?
  67. TRENA        EQU    $0C    ; Transmit, Receive ENAble
  68. RDRF        EQU    $20    ; Receive Data Register Full
  69. TDRE        EQU    $80    ; Transmit Data Register Empty
  70.  
  71. ********************************************************************************
  72. * zero page RAM definitions. Do not use FCB here. It will stomp EEBOOT20.
  73. ********************************************************************************
  74.  
  75.     ORG    $00        ; The beginning of RAM
  76. values  RMB    8        ; The place where the unsorted values live
  77. servnum    RMB    2        ; A safe place for the servo number
  78. smasks    RMB    8        ; A safe place to put sorted masks
  79. svalues    RMB    8        ; A safe place to put sorted values
  80. dvalues    RMB    8        ; A safe place to put sorted delta values
  81. newvals    RMB    1        ; Alert the interrupt routine about new values
  82.  
  83. **********************************************************************
  84. *                   MAIN CODE                 *
  85. **********************************************************************
  86.  
  87.     ORG    $F800        ; $F800 is the beginning of EEPROM
  88. *                ; on a MC68HC811E2 processor
  89.  
  90. Start:
  91.     LDS    #$00FF        ; Set stack at the top of ram
  92.  
  93.     BCLR    SPCR PORTD_WOM     ; initialize serial port
  94.     LDAA    #BAUD9600    ; turn off wire-or mode
  95.     STAA    BAUD        ; Set the port for 9600 baud
  96.     LDAA    #TRENA
  97.     STAA    SCCR2        ; Enable the serial subsystem
  98.  
  99.     LDAA    #1
  100.     STAA    newvals        ; Set newvals the first time through
  101.  
  102.     LDAA    #0
  103.     STAA    servnum        ; Zero out the high byte of servnum
  104.  
  105.     LDX    #0        ; set the servos to the middle
  106.     LDAA    #$80        ; of the range of pulse widths
  107. Stlp:
  108.     STAA    values,X
  109.     INX
  110.     CPX    #7
  111.     BLS    Stlp
  112.  
  113. ********************************************************************************
  114. * set up interrupts
  115. * OC2:  once every 20 milliseconds
  116. ********************************************************************************
  117.  
  118.     LDAA    #%01000000    ; Set up the OC2 interrupt to generate
  119.     STAA    TCTL1        ; an interrupt once every 
  120.     STAA    TFLG1         ; 20 milliseconds
  121.     STAA    TMSK1
  122.     CLI            ; Turn on interrupts
  123.  
  124. mainloop:
  125.     LDAA    #$0D        ; return character
  126.     JSR    putchar
  127.     LDAA    #$0A        ; linefeed character
  128.     JSR    putchar
  129.     LDAA    #'>        ; prompt character
  130.     JSR    putchar
  131.  
  132. cmd_get_type:
  133.     JSR    getchar        ; Keep getting chars until you get one
  134.     CMPA    #'a        ; that is alphabetic.
  135.     BLO    cmd_get_type    ; If the char was less than 'a' ignore
  136.  
  137.       CMPA    #'s        ; The command is an 's' set a new value
  138.     BEQ    set_servo_val
  139.     CMPA    #'r        ; the command is an 'r' reset values
  140.     BEQ     reset_servos
  141.     BRA    mainloop    ; Do it all over again
  142.  
  143. set_servo_val:
  144.     JSR    getbyte        ; Get the servo number
  145.     STAA    servnum+1
  146.     LDX    servnum        ; And get it into the X register
  147.     JSR    getbyte        ; Get the pulse width
  148.     STAA    values,X    ; And save it in the values list
  149.     LDAA    #1        ; Alert the interrupt handler that there are
  150.     STAA    newvals        ; new values in the values list
  151.     BRA    mainloop    ; Go back to the command loop
  152.  
  153. reset_servos
  154.     LDX    #0
  155. rtop:
  156.     LDAA    #$80        ; Reset the servo values to a nice middle
  157.     STAA    values,X    ; pulse width
  158.     INX
  159.     CPX    #7
  160.     BLS    rtop
  161.         BRA    mainloop    ; Go back to the command loop
  162.  
  163. getbyte:
  164.     JSR    getchar        ; read 2 chars from the serial port and change
  165.         CMPA    #'A        ; them into a one byte value in accumulator A
  166.         BLO     hibyteok    ; This routine destroys accumulator B
  167.         SUBA    #'A-10        ; so watch it.
  168. hibyteok
  169.     ASLA
  170.     ASLA
  171.     ASLA
  172.     ASLA
  173.     TAB
  174.     JSR    getchar        ; Get the second character which is the least
  175.         CMPA    #'A        ; significant 4 bits of the value
  176.         BLO     lobyteok
  177.         SUBA    #'A-10
  178. lobyteok
  179.     ANDA    #$0f
  180.     ABA
  181.     RTS
  182.  
  183. getchar:
  184.     LDAA    SCSR        ; Read a character from the serial port and put
  185.     ANDA    #RDRF        ; it in accumulator A
  186.         BEQ    getchar
  187.     LDAA    SCDR
  188.         ANDA    #$7f
  189.     RTS
  190.  
  191. putchar:
  192.     LDAB    SCSR        ; Send the character in accumulator A out the
  193.      ANDB    #TDRE        ; serial port.
  194.     BEQ    putchar        ; This routine destroys accumulator B
  195.     STAA    SCDR        ; so watch it.
  196.     RTS
  197.  
  198. oc2int:
  199.     LDD    #40000        ; Once every 20 milliseconds
  200.     ADDD    TOC2        ; We need to generate an interrupt
  201.     STD    TOC2
  202.     LDX    #$1000
  203.     BCLR    RTFLG1,X %10111111    ; clear OC2 for next compare
  204.     
  205. ********************************************************************************
  206. * This section is timing critical. If you need to change anything in the
  207. * oc2st loop you MUST make sure to get the delay in the oc2dn loop identical.
  208. * There is a delay of 675 clocks to set the minimum pulse width. If you want
  209. * to have a longer or shorter minimum pulse width you can change this value.
  210. * The value 675 is derived as follows: The oc2st loop takes 39 clocks to
  211. * start each pulse. Therefore it takes 7*39 clocks from the start of the first 
  212. * pulse until the loop completes. The oc2dn loop takes 32 clocks until it stops
  213. * the first pulse if that servo's values array holds a zero. This means that if
  214. * we want a 950 clock (475 microsecond) minimum pulse we need to delay
  215. * 950 - (7*39) - 32 = 645 clocks between the end of oc2st and the beginning
  216. * of oc2dn. A DECA/BNE loop takes 5 clocks so we load A with 645/5 = 129.
  217. * Now the LDAA #129 adds 2 clocks to the delay but do you REALLY care about
  218. * a 2 clock difference in the desired minimum pulse width and the actual
  219. * minimum pulse width?
  220. * Do NOT replace it with an interrupt driven delay since this would introduce
  221. * an unpredictable interrupt latency of as much as 41 clocks. This much change
  222. * in the pulse width will almost surely cause the servos to jitter.
  223. * Currently the oc2st loop takes 39 clocks as does the oc2dn loop.
  224. * The pulse width changes 13 clocks for every count in the values array.
  225. * This gives you a 13*256*500nanoSecond = 1664 microsecond change in pulse
  226. * width from a zero to FF in the values array. If you add the 950 clock minimum
  227. * pulse width to that you can produce pulses from 475 microseconds through
  228. * 2139 microseconds with a resolution of 6.5 microseconds. Pretty good huh?
  229. * The numbers after the semicolon are the number of clocks the instruction
  230. * takes to execute. Branches take the same time even if the branch is not taken.
  231. * One clock is 500 nanoseconds if you are using an 8 mHz crystal. If you are
  232. * not using an 8 mHz crystal then all these timings are wrong. Too bad.
  233. ********************************************************************************
  234. oc2st:
  235.     LDX    #0        ;3 Start at the shortest pulse-width
  236. oc2st1:
  237.     LDAB    smasks,X    ;4 Figure out which servo it is
  238.     ORAB    PORTB        ;4
  239.     STAB    PORTB        ;4 Turn it on
  240.     LDAA    #3        ;2 We need to blow 15 clocks
  241. oc2st2:
  242.     DECA            ;2
  243.     BNE    oc2st2        ;3
  244.     INX            ;3 Go to the next servo
  245.     CPX    #7        ;4
  246.     BLS    oc2st1        ;3 Not done, do another one.
  247.  
  248.     LDAA    #129        ;2 Now, delay the minimum pulse-width
  249. delay1:
  250.     DECA            ;2 Which is 645 clocks
  251.     BNE    delay1        ;3
  252.  
  253. oc2dn:
  254.     LDX    #0        ;3 Now start turning off the servo pulses
  255. oc2dn1:
  256.     LDAB    smasks,X    ;4 Figure out which servo is first
  257.     EORB    PORTB        ;4 Figure out what to store in PORTB
  258.     LDAA    dvalues,X    ;4 Get this servos desired pulse width
  259. oc2dn2:
  260.     NOP            ;2
  261.     NOP            ;2
  262.     NOP            ;2
  263.     NOP            ;2
  264.     DECA            ;2 And delay 13 clocks per unit
  265.     BNE    oc2dn2        ;3
  266.     STAB    PORTB        ;4 Finally turn off the pulse
  267.     INX            ;3 Go to the next servo
  268.     CPX    #7        ;4 Are we on the last servo?
  269.     BLS    oc2dn1        ;3 No, do another one
  270.  
  271.     LDAA    newvals        ; Next see if there is a new values list
  272.     CMPA    #0
  273.     BEQ    done        ; If not, we are done
  274.  
  275.     LDAA    #0
  276.     STAA    newvals        ; Zero out the new values indicator
  277.  
  278.     LDX    #0
  279.     LDAB    #$01
  280. oc2cp:
  281.     LDAA    values,X    ; Copy the values array
  282.     STAA    svalues,X    ; Into the svalues array
  283.     STAB    smasks,X    ; (Save a proper mask for this port)
  284.     ASLB            ; 
  285.     INX            ; So that we can sort them without
  286.     CPX    #7        ; worrying about getting new user
  287.     BLS    oc2cp        ; values in the values array
  288.  
  289. ********************************************************************************
  290. * Sort the svalues list so that the lowest pulse widths are at the beginning of
  291. * the list. Also make sure to swap around the smasks list so that the proper
  292. * masks stay with the values.
  293. ********************************************************************************
  294. oc2sort:
  295.     LDY    #0        ; Y is our "times through the list" counter
  296. sorttop:
  297.     LDX    #0        ; X is our pointer into the list of values
  298. sortlp:
  299.     LDAA    svalues,X    ; Get a value
  300.     LDAB    svalues+1,X    ; Get the following value
  301.     CBA            ; Compare them
  302.     BLS    noswap        ; The first one is lower. No not swap them
  303.     STAA    svalues+1,X    ; Swap the two values
  304.     STAB    svalues,X
  305.     LDAA    smasks,X    ; Also swap the bit masks
  306.     LDAB    smasks+1,X
  307.     STAA    smasks+1,X
  308.     STAB    smasks,X
  309. noswap:
  310.     INX            ; Go to the next entry in the list
  311.     CPX    #6        ; 6 is the second-to-last item and we are done
  312.     BLS    sortlp        ; Not done yet, do another pair of items
  313.     INY
  314.     CPY    #6        ; We only need to sort 7 times so 6 is right
  315.     BLS    sorttop
  316.  
  317.     LDX    #0        ; Now convert from pulse width values
  318.     LDAA    svalues,X
  319.     INCA            ; (Fix up for the delay loop)
  320.     STAA    dvalues,X
  321. oc2con:
  322.     LDAA    svalues+1,X    ; Into delta values so that we can simply
  323.     SUBA    svalues,X    ; delay the time left for each pulse
  324.     INCA            ; (Fix up for the delay loop)
  325.     STAA    dvalues+1,X
  326.     INX
  327.     CPX    #6
  328.     BLS    oc2con
  329. done:
  330.     RTI            ; Done, now that didn't hurt too much did it?
  331.  
  332. BadInt    RTI            ; Set all unused vectors here
  333.  
  334.     Org    $FFC0        ; Where the interrupt vectors are
  335.  
  336.     FDB    BadInt    * $FFC0    ; Reserved
  337.     FDB    BadInt    * $FFC2    ; Reserved
  338.     FDB    BadInt    * $FFC4    ; Reserved
  339.     FDB    BadInt    * $FFC6    ; Reserved
  340.     FDB    BadInt    * $FFC8    ; Reserved
  341.     FDB    BadInt    * $FFCA    ; Reserved
  342.     FDB    BadInt    * $FFCC    ; Reserved
  343.     FDB    BadInt    * $FFCE    ; Reserved
  344.     FDB    BadInt    * $FFD0    ; Reserved
  345.     FDB    BadInt    * $FFD2    ; Reserved
  346.     FDB    BadInt    * $FFD4    ; Reserved
  347.  
  348.     FDB    BadInt    * $FFD6    ; SCI Serial System
  349.     FDB    BadInt    * $FFD8    ; SPI Serial Transfer Complete
  350.     FDB    BadInt    * $FFDA    ; Pulse Accumulator Input Edge
  351.     FDB    BadInt    * $FFDC    ; Pulse Accumulator Overflow
  352.     FDB    BadInt    * $FFDE    ; Timer Overflow
  353.     FDB    BadInt    * $FFE0    ; In Capture 4/Output Compare 5 (TI4O5)
  354.     FDB    BadInt  * $FFE2    ; Timer Output Compare 4 (TOC4)
  355.     FDB    BadInt    * $FFE4    ; Timer Output Compare 3 (TOC3) 
  356.     FDB    oc2int    * $FFE6    ; Timer Output Compare 2 (TOC2)
  357.     FDB    BadInt    * $FFE8    ; Timer Output Compare 1 (TOC1)
  358.     FDB    BadInt    * $FFEA    ; Timer Input Capture 3 (TIC3)
  359.     FDB    BadInt    * $FFEC    ; Timer Input Capture 2 (TIC2)
  360.     FDB    BadInt    * $FFEE    ; Timer Input Capture 1 (TIC1)
  361.     FDB    BadInt    * $FFF0    ; Real Time Interrupt (RTI)
  362.     FDB    BadInt    * $FFF2    ; External Pin or Parallel I/O (IRQ)
  363.     FDB    BadInt    * $FFF4    ; Pseudo Non-Maskable Interrupt (XIRQ)
  364.     FDB    BadInt    * $FFF6    ; Software Interrupt (SWI)
  365.     FDB    BadInt    * $FFF8    ; Illegal Opcode Trap ()
  366.     FDB    BadInt    * $FFFA    ; COP Failure (Reset) ()
  367.     FDB     BadInt    * $FFFC    ; COP Clock Monitor Fail (Reset) ()
  368.     FDB    Start    * $FFFE    ; /RESET
  369.     END
  370.